/**********************************************************************
 *      Author: tstern
 *
 *	Misfit Tech invests time and resources providing this open source code,
 *	please support Misfit Tech and open-source hardware by purchasing
 *	products from Misfit Tech, www.misifittech.net!
 *
 *	Written by Trampas Stern  for Misfit Tech.
 *	BSD license, check license.txt for more information
 *	All text above, must be included in any redistribution
 *********************************************************************/
#include "stepper_controller.h"

#include "nonvolatile.h" //for programmable parameters
#include <Wire.h>
#include <inttypes.h>

#pragma GCC push_options
#pragma GCC optimize ("-Ofast")

#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY);

volatile bool TC5_ISR_Enabled=false;

void setupTCInterrupts() {



   // Enable GCLK for TC4 and TC5 (timer counter input clock)
   GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5));
   while (GCLK->STATUS.bit.SYNCBUSY);

   TC5->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE;   // Disable TCx
   WAIT_TC16_REGS_SYNC(TC5)                      // wait for sync

   TC5->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;   // Set Timer counter Mode to 16 bits
   WAIT_TC16_REGS_SYNC(TC5)

   TC5->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC as normal Normal Frq
   WAIT_TC16_REGS_SYNC(TC5)

   TC5->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1;   // Set perscaler
   WAIT_TC16_REGS_SYNC(TC5)


   TC5->COUNT16.CC[0].reg = F_CPU/NZS_CONTROL_LOOP_HZ;
   WAIT_TC16_REGS_SYNC(TC5)


   TC5->COUNT16.INTENSET.reg = 0;              // disable all interrupts
   TC5->COUNT16.INTENSET.bit.OVF = 1;          // enable overfollow
   //  TC5->COUNT16.INTENSET.bit.MC0 = 1;         // enable compare match to CC0


   NVIC_SetPriority(TC5_IRQn, 2);


   // Enable InterruptVector
   NVIC_EnableIRQ(TC5_IRQn);


   // Enable TC
   TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
   WAIT_TC16_REGS_SYNC(TC5)

}

static void enableTCInterrupts() {

   TC5_ISR_Enabled=true;
   NVIC_EnableIRQ(TC5_IRQn);
   TC5->COUNT16.INTENSET.bit.OVF = 1;
   //  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;    //Enable TC5
   //  WAIT_TC16_REGS_SYNC(TC5)                      //wait for sync
}

static void disableTCInterrupts() {

   TC5_ISR_Enabled=false;
   //NVIC_DisableIRQ(TC5_IRQn);
   TC5->COUNT16.INTENCLR.bit.OVF = 1;
}

static bool enterCriticalSection()
{
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   return state;
}

static void exitCriticalSection(bool prevState)
{
   if (prevState)
   {
      enableTCInterrupts();
   } //else do nothing
}


void StepperCtrl::updateParamsFromNVM(void)
{
   bool state=enterCriticalSection();

   pPID.Kd=NVM->pPID.Kd*CTRL_PID_SCALING;
   pPID.Ki=NVM->pPID.Ki*CTRL_PID_SCALING;
   pPID.Kp=NVM->pPID.Kp*CTRL_PID_SCALING;

   vPID.Kd=NVM->vPID.Kd*CTRL_PID_SCALING;
   vPID.Ki=NVM->vPID.Ki*CTRL_PID_SCALING;
   vPID.Kp=NVM->vPID.Kp*CTRL_PID_SCALING;

   sPID.Kd=NVM->sPID.Kd*CTRL_PID_SCALING;
   sPID.Ki=NVM->sPID.Ki*CTRL_PID_SCALING;
   sPID.Kp=NVM->sPID.Kp*CTRL_PID_SCALING;

   if (NVM->SystemParams.parametersVaild)
   {
      memcpy((void *)&systemParams, (void *)&NVM->SystemParams, sizeof(systemParams));
   }else
   {
      ERROR("This should never happen but just in case");
      systemParams.microsteps=16;
      systemParams.controllerMode=CTRL_SIMPLE;
      systemParams.dirPinRotation=CW_ROTATION; //default to clockwise rotation when dir is high
      systemParams.errorLimit=(int32_t)ANGLE_FROM_DEGREES(1.8);
      systemParams.errorPinMode=ERROR_PIN_MODE_ENABLE;  //default to enable pin
   }

   //default the error pin to input, if it is an error pin the
   // handler for this will change the pin to be an output.
   // for bidirection error it has to handle input/output it's self as well.
   // This is not the cleanest way to handle this...
   // TODO implement this cleaner?
   pinMode(PIN_ERROR, INPUT_PULLUP); //we have input pin

   if (NVM->motorParams.parametersVaild)
   {
      memcpy((void *)&motorParams, (void *)&NVM->motorParams, sizeof(motorParams));
   } else
   {
      //MotorParams_t Params;
      motorParams.fullStepsPerRotation=200;
      motorParams.currentHoldMa=500;
      motorParams.currentMa=1500;
      motorParams.motorWiring=true;
      //memcpy((void *)&Params, (void *)&motorParams, sizeof(motorParams));
      //nvmWriteMotorParms(Params);
   }

   stepperDriver.setRotationDirection(motorParams.motorWiring);

   exitCriticalSection(state);
}


void  StepperCtrl::motorReset(void)
{
   //when we reset the motor we want to also sync the motor
   // phase.  Therefore we move forward a few full steps then back
   // to sync motor phasing, leaving the motor at "phase 0"
   bool state=enterCriticalSection();

   //	stepperDriver.move(0,motorParams.currentMa);
   //	delay(100);
   //	stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa);
   //	delay(100);
   //	stepperDriver.move(A4954_NUM_MICROSTEPS*2,motorParams.currentMa);
   //	delay(100);
   //	stepperDriver.move(A4954_NUM_MICROSTEPS*3,motorParams.currentMa);
   //	delay(100);
   //	stepperDriver.move(A4954_NUM_MICROSTEPS*2,motorParams.currentMa);
   //	delay(100);
   //	stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa);
   //	delay(100);
   stepperDriver.move(0,motorParams.currentMa);
   delay(1000);

   setLocationFromEncoder(); //measure new starting point
   exitCriticalSection(state);
}

void StepperCtrl::setLocationFromEncoder(void)
{
   numSteps=0;
   currentLocation=0;

   if (calTable.calValid())
   {
      int32_t n,x;
      int32_t calIndex;
      Angle a;

      //set our angles based on previous cal data

      x=sampleMeanEncoder(200);
      a=calTable.fastReverseLookup(x);

      //our cal table starts at angle zero, so lets set starting based on this and stepsize
      LOG("start angle %d, encoder %d", (uint16_t)a,x);

      //TODO we need to handle 0.9 degree motor
      if (CALIBRATION_TABLE_SIZE == motorParams.fullStepsPerRotation)
      {
	 n=(int32_t)ANGLE_STEPS/CALIBRATION_TABLE_SIZE;

	 calIndex=((int32_t)((uint16_t)a+n/2)*CALIBRATION_TABLE_SIZE)/ANGLE_STEPS; //find calibration index
	 if (calIndex>CALIBRATION_TABLE_SIZE)
	 {
	    calIndex-=CALIBRATION_TABLE_SIZE;
	 }
	 a=(uint16_t)((calIndex*ANGLE_STEPS)/CALIBRATION_TABLE_SIZE);
      }


      x=(int32_t)((((float)(uint16_t)a)*360.0/(float)ANGLE_STEPS)*1000);
      LOG("start angle after rounding %d %d.%03d", (uint16_t)a,x/1000,x%1000);

      //we need to set our numSteps
      numSteps=DIVIDE_WITH_ROUND( ((int32_t)a *motorParams.fullStepsPerRotation*systemParams.microsteps),ANGLE_STEPS);
      currentLocation=(uint16_t)a;
   }
   zeroAngleOffset=getCurrentLocation();
}

int64_t StepperCtrl::getZeroAngleOffset(void)
{
   int64_t x;
   bool state=enterCriticalSection();

   x=zeroAngleOffset;

   exitCriticalSection(state);
   return x;
}

void StepperCtrl::setAngle(int64_t angle)
{
   bool state=enterCriticalSection();

   zeroAngleOffset=getCurrentLocation()-angle;

   exitCriticalSection(state);
}

void StepperCtrl::setZero(void)
{
   //we want to set the starting angle to zero.
   bool state=enterCriticalSection();

   zeroAngleOffset=getCurrentLocation();

   exitCriticalSection(state);
}

void StepperCtrl::encoderDiagnostics(char *ptrStr)
{
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();

   encoder.diagnostics(ptrStr);

   if (state) enableTCInterrupts();
}


//TODO This function does two things, set rotation direction
//  and measures step size, it should be two functions.
//return is anlge in degreesx100 ie 360.0 is returned as 36000
float StepperCtrl::measureStepSize(void)
{
   int32_t angle1,angle2,x,i;
   bool feedback=enableFeedback;
   int32_t microsteps=systemParams.microsteps;

   systemParams.microsteps=1;
   enableFeedback=false;
   motorParams.motorWiring=true; //assume we are forward wiring to start with
   stepperDriver.setRotationDirection(motorParams.motorWiring);
   /////////////////////////////////////////
   //// Measure the full step size /////
   /// Note we assume machine can take one step without issue///

   LOG("reset motor");
   motorReset(); //this puts stepper motor at stepAngle of zero

   LOG("sample encoder");

   angle1=sampleMeanEncoder(200);

   LOG("move");
   stepperDriver.move(A4954_NUM_MICROSTEPS/2,motorParams.currentMa); //move one half step 'forward'
   delay(100);
   stepperDriver.move(A4954_NUM_MICROSTEPS,motorParams.currentMa); //move one half step 'forward'
   delay(500);
   LOG("sample encoder");
   angle2=sampleMeanEncoder(200);

   LOG("Angles %d %d",angle1,angle2);
   if ((abs(angle2-angle1))>(ANGLE_STEPS/2))
   {
      //we crossed the wrap around
      if (angle1>angle2)
      {
	 angle1=angle1+(int32_t)ANGLE_STEPS;
      }else
      {
	 angle2=angle2+(int32_t)ANGLE_STEPS;
      }
   }
   LOG("Angles %d %d",angle1,angle2);

   //when we are increase the steps in the  stepperDriver.move() command
   // we want the encoder increasing. This ensures motor is moving clock wise
   // when encoder is increasing.
   //	if (angle2>angle1)
   //	{
   //		motorParams.motorWiring=true;
   //		stepperDriver.setRotationDirection(true);
   //		LOG("Forward rotating");
   //	}else
   //	{
   //		//the motor is wired backwards so correct in stepperDriver
   //		motorParams.motorWiring=false;
   //		stepperDriver.setRotationDirection(false);
   //		LOG("Reverse rotating");
   //	}
   x=((int64_t)(angle2-angle1)*36000)/(int32_t)ANGLE_STEPS;
   // if x is ~180 we have a 1.8 degree step motor, if it is ~90 we have 0.9 degree step
   LOG("%angle delta %d %d (%d %d)",x,abs(angle2-angle1),angle1,angle2 );

   //move back
   stepperDriver.move(-A4954_NUM_MICROSTEPS/2,motorParams.currentMa); //move one half step 'forward'
   delay(100);
   stepperDriver.move(-A4954_NUM_MICROSTEPS,motorParams.currentMa); //move one half step 'forward'

   systemParams.microsteps=microsteps;
   enableFeedback=feedback;

   return ((float)x)/100.0;
}


int32_t StepperCtrl::measureError(void)
{
   //LOG("current %d desired %d %d",(int32_t) currentLocation, (int32_t)getDesiredLocation(), numSteps);

   return ((int32_t)currentLocation-(int32_t)getDesiredLocation());
}

/*
bool StepperCtrl::changeMicrostep(uint16_t microSteps)
{
	bool state=TC5_ISR_Enabled;
	disableTCInterrupts();
	systemParams.microsteps=microSteps;
	motorReset();
	if (state) enableTCInterrupts();
	return true;
}
 */
Angle StepperCtrl::maxCalibrationError(void)
{
   //Angle startingAngle;
   bool done=false;
   int32_t mean;
   int32_t maxError=0, j;
   int16_t dist;
   uint16_t angle=0;
   bool feedback=enableFeedback;
   uint16_t microSteps=systemParams.microsteps;
   int32_t steps;
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();


   if (false == calTable.calValid())
   {
      return ANGLE_MAX;
   }

   enableFeedback=false;
   j=0;
   LOG("Running calibration test");

   systemParams.microsteps=1;
   motorReset();
   steps=0;

   while(!done)
   {
      Angle cal;
      Angle act, desiredAngle;

      //todo we should measure mean and wait until stable.
      delay(200);
      mean=sampleMeanEncoder(200);
      desiredAngle=(uint16_t)(getDesiredLocation() & 0x0FFFFLL);

      cal=calTable.getCal(desiredAngle);
      dist=Angle(mean)-cal;
      act=calTable.fastReverseLookup(cal);

      LOG("actual %d, cal %d",mean,(uint16_t)cal);
      LOG("desired %d",(uint16_t)desiredAngle);
      LOG("numSteps %d", numSteps);

      LOG("cal error for step %d is %d",j,dist);
      LOG("mean %d, cal %d",mean, (uint16_t)cal);

      updateStep(0,1);

      // move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
      steps+=A4954_NUM_MICROSTEPS/2;
      stepperDriver.move(steps,motorParams.currentMa);

      delay(50);
      steps+=A4954_NUM_MICROSTEPS/2;
      stepperDriver.move(steps,motorParams.currentMa);


      if (400==motorParams.fullStepsPerRotation)
      {
	 delay(100);
	 updateStep(0,1);
	 // move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
	 steps+=A4954_NUM_MICROSTEPS/2;
	 stepperDriver.move(steps,motorParams.currentMa);

	 delay(100);
	 steps+=A4954_NUM_MICROSTEPS/2;
	 stepperDriver.move(steps,motorParams.currentMa);
      }
      //delay(400);

      if (abs(dist)>maxError)
      {
	 maxError=abs(dist);
      }

      j++;
      if (j>=(1*CALIBRATION_TABLE_SIZE+3))
      {
	 done=true;
      }


   }
   systemParams.microsteps=microSteps;
   motorReset();
   enableFeedback=feedback;
   if (state) enableTCInterrupts();
   LOG("max error is %d cnts", maxError);
   return Angle((uint16_t)maxError);
}


//The encoder needs to be calibrated to the motor.
// we will assume full step detents are correct,
// ex 1.8 degree motor will have 200 steps for 360 degrees.
// We also need to calibrate the phasing of the motor
// to the A4954. This requires that the A4954 "step angle" of
// zero is the first entry in the calibration table.
bool StepperCtrl::calibrateEncoder(void)
{
   int32_t x,i,j;
   uint32_t angle=0;
   int32_t steps;
   bool done=false;

   int32_t mean;
   uint16_t microSteps=systemParams.microsteps;
   bool feedback=enableFeedback;
   bool state=TC5_ISR_Enabled;

   disableTCInterrupts();

   enableFeedback=false;
   systemParams.microsteps=1;
   LOG("reset motor");
   motorReset();
   LOG("Starting calibration");
   delay(200);
   steps=0;
   j=0;
   while(!done)
   {
      int ii,N;
      Angle cal,desiredAngle;
      desiredAngle=(uint16_t)(getDesiredLocation() & 0x0FFFFLL);
      cal=calTable.getCal(desiredAngle);
      delay(200);
      mean=sampleMeanEncoder(200);

      LOG("Previous cal distance %d, %d, mean %d, cal %d",j, cal-Angle((uint16_t)mean), mean, (uint16_t)cal);

      calTable.updateTableValue(j,mean);

      updateStep(0,1);

      N=2;
      // move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
      for (ii=0; ii<N; ii++)
      {
	 steps+=A4954_NUM_MICROSTEPS/N;
	 stepperDriver.move(steps,motorParams.currentMa);

	 delay(50);
      }
      //steps+=A4954_NUM_MICROSTEPS/2;
      //stepperDriver.move(steps,motorParams.currentMa);



      if (400==motorParams.fullStepsPerRotation)
      {
	 delay(100);
	 updateStep(0,1);
	 // move one half step at a time, a full step move could cause a move backwards depending on how current ramps down
	 steps+=A4954_NUM_MICROSTEPS/2;
	 stepperDriver.move(steps,motorParams.currentMa);

	 delay(100);
	 steps+=A4954_NUM_MICROSTEPS/2;
	 stepperDriver.move(steps,motorParams.currentMa);

      }

      j++;
      if (j>=CALIBRATION_TABLE_SIZE)
      {
	 done=true;
      }


   }
   //calTable.printCalTable();
   //calTable.smoothTable();
   //calTable.printCalTable();
   calTable.saveToFlash(); //saves the calibration to flash
   calTable.printCalTable();

   systemParams.microsteps=microSteps;
   motorReset();
   enableFeedback=feedback;
   if (state) enableTCInterrupts();
   return done;
}





stepCtrlError_t StepperCtrl::begin(void)
{
   int i;
   float x;


   enableFeedback=false;
   velocity=0;
   currentLocation=0;
   numSteps=0;

   //we have to update from NVM before moving motor
   updateParamsFromNVM(); //update the local cache from the NVM

   LOG("start up encoder");
   if (false == encoder.begin(PIN_AS5047D_CS))
   {
      return STEPCTRL_NO_ENCODER;
   }

   LOG("cal table init");
   calTable.init();

   startUpEncoder=(uint16_t)getEncoderAngle();
   WARNING("start up encoder %d",startUpEncoder);

   LOG("start stepper driver");
   stepperDriver.begin();


   LOG("measuring step size");
   x=measureStepSize();
   if (abs(x)<0.5)
   {
      ERROR("Motor may not have power");
      return STEPCTRL_NO_POWER;
   }



   LOG("Checking the motor parameters");
   //todo we might want to move this up a level to the NZS
   //  especially since it has default values
   if (false == NVM->motorParams.parametersVaild)
   {
      MotorParams_t params;
      WARNING("NVM motor parameters are not set, we will update");

      //power could have just been applied and step size read wrong
      // if we are more than 200 steps/rotation which is most common
      // lets read again just to be sure.
      if (abs(x)<1.5)
      {
	 //run step test a second time to be sure
	 x=measureStepSize();
      }

      if (x>0)
      {
	 motorParams.motorWiring=true;
      } else
      {
	 motorParams.motorWiring=false;
      }
      if (abs(x)<=1.2)
      {
	 motorParams.fullStepsPerRotation=400;
      }else
      {
	 motorParams.fullStepsPerRotation=200;
      }

      memcpy((void *)&params, (void *)&motorParams,sizeof(motorParams));
      nvmWriteMotorParms(params);
   }

   LOG("Motor params are now good");
   LOG("fullSteps %d", motorParams.fullStepsPerRotation);
   LOG("motorWiring %d", motorParams.motorWiring);
   LOG("currentMa %d", motorParams.currentMa);
   LOG("holdCurrentMa %d", motorParams.currentHoldMa);


   updateParamsFromNVM(); //update the local cache from the NVM


   if (false == calTable.calValid())
   {
      return STEPCTRL_NO_CAL;
   }


   enableFeedback=true;
   setupTCInterrupts();
   enableTCInterrupts();
   return STEPCTRL_NO_ERROR;

}

Angle StepperCtrl::sampleAngle(void)
{
   uint16_t angle;
   int32_t x,y;

#ifdef NZS_AS5047_PIPELINE
   //read encoder twice such that we get the latest sample as the pipeline is always once sample behind


   y=encoder.readEncoderAnglePipeLineRead(); //convert the 14 bit encoder value to a 16 bit number
   x=encoder.readEncoderAnglePipeLineRead();


   angle=((uint32_t)(x)*4); //convert the 14 bit encoder value to a 16 bit number
#else
   angle=((uint32_t)encoder.readEncoderAngle())<<2; //convert the 14 bit encoder value to a 16 bit number
#endif
   return Angle(angle);
}

//when sampling the mean of encoder if we are on roll over
// edge we can have an issue so we have this function
// to do the mean correctly
Angle StepperCtrl::sampleMeanEncoder(int32_t numSamples)
{

   int32_t i,last,x=0;
   int64_t mean=0;
   int32_t min,max;

   mean=0;
   for (i=0; i<(numSamples); i++)
   {
      int32_t d;
      last=x;
      x=(((int32_t)encoder.readEncoderAngle())*4);
      if (encoder.getError())
      {

	 SerialUSB.println("AS5047 Error");
	 delay(1000);
	 return 0;
      }
      if(i==0)
      {
	 last=x;
	 min=x;
	 max=x;
      }
      //LOG("i %d,min %d, max %d, last %d, x %d", i, min, max, last, x);
      if (abs(last-x)>65536/2)
      {
	 if (last>x)
	 {
	    x=x+65536;
	 } else
	 {
	    x=x-65536;
	 }
      }


      if (x>max)
      {
	 max=x;
      }
      if (x<min)
      {
	 min=x;
      }

      mean=mean+(x);
   }

   LOG("min %d, max %d, mean %d", min, max, (int32_t)(mean/numSamples));
   return Angle(mean/numSamples);
}

void StepperCtrl::feedback(bool enable)
{
   disableTCInterrupts();
   motorReset();
   enableFeedback=enable;
   if (enable == true)
   {
      enableTCInterrupts();
   }
}

void StepperCtrl::updateStep(int dir, uint16_t steps)
{
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   if (dir)
   {
      numSteps-=steps;
   }else
   {
      numSteps+=steps;
   }
   if (state) enableTCInterrupts();
}

void StepperCtrl::requestStep(int dir, uint16_t steps)
{
   bool state;
   state=TC5_ISR_Enabled;
   disableTCInterrupts();

   if (dir)
   {
      numSteps-=steps;
   }else
   {
      numSteps+=steps;
   }

   if (false == enableFeedback)
   {
      moveToAngle(getDesiredLocation(),motorParams.currentMa);
   }
   if (state) enableTCInterrupts();
}


void StepperCtrl::move(int dir, uint16_t steps)
{
   int64_t ret;
   int32_t n;



   updateStep(dir,steps);

   if (false == enableFeedback)
   {
      n=systemParams.microsteps;
      ret=((int64_t)numSteps * A4954_NUM_MICROSTEPS+(n/2))/n;
      n=A4954_NUM_MICROSTEPS*motorParams.fullStepsPerRotation;
      while(ret>n)
      {
	 ret-=n;
      }
      while(ret<-n)
      {
	 ret+=n;
      }
      n=(int32_t)(ret);
      LOG("s is %d %d",n,steps);
      stepperDriver.move(n,motorParams.currentMa);
   }


}



int64_t StepperCtrl::getDesiredLocation(void)
{
   int64_t ret;
   int32_t n;
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   n=motorParams.fullStepsPerRotation * systemParams.microsteps;
   ret=((int64_t)numSteps * (int64_t)ANGLE_STEPS+(n/2))/n ;
   if (state) enableTCInterrupts();
   return ret;
}




void StepperCtrl::moveToAbsAngle(int32_t a)
{

   int64_t ret;
   int32_t n;


   n=motorParams.fullStepsPerRotation * systemParams.microsteps;

   ret=(((int64_t)a+zeroAngleOffset)*n+ANGLE_STEPS/2)/(int32_t)ANGLE_STEPS;
   bool state=enterCriticalSection();
   numSteps=ret;
   exitCriticalSection(state);
}

void StepperCtrl::moveToAngle(int32_t a, uint32_t ma)
{
   //we need to convert 'Angle' to A4954 steps
   a=a % ANGLE_STEPS;  //we only interested in the current angle


   a=DIVIDE_WITH_ROUND( (a*motorParams.fullStepsPerRotation*A4954_NUM_MICROSTEPS), ANGLE_STEPS);

   //LOG("move %d %d",a,ma);
   stepperDriver.move(a,ma);

}

Angle StepperCtrl::getEncoderAngle(void)
{
   Angle a;
   bool state=enterCriticalSection();
   a=calTable.fastReverseLookup(sampleAngle());
   exitCriticalSection(state);
   return a;
}

int64_t StepperCtrl::getCurrentLocation(void)
{
   Angle a;
   int32_t x;
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   a=calTable.fastReverseLookup(sampleAngle());
   x=(int32_t)a - (int32_t)((currentLocation) & ANGLE_MAX);

   if (x>((int32_t)ANGLE_STEPS/2))
   {
      currentLocation -= ANGLE_STEPS;
   }
   if (x<-((int32_t)ANGLE_STEPS/2))
   {
      currentLocation += ANGLE_STEPS;
   }
   currentLocation=(currentLocation & 0xFFFFFFFFFFFF0000UL) | (uint16_t)a;
   if (state) enableTCInterrupts();
   return currentLocation;

}



int64_t StepperCtrl::getCurrentAngle(void)
{
   int64_t x;
   x=getCurrentLocation()-zeroAngleOffset;
   return x;
}


int64_t StepperCtrl::getDesiredAngle(void)
{
   int64_t x;
   x=getDesiredLocation()-zeroAngleOffset;
   return x;
}

void StepperCtrl::setVelocity(int64_t vel)
{
   bool state=enterCriticalSection();

   velocity=vel;
   exitCriticalSection(state);
}

int64_t StepperCtrl::getVelocity(void)
{
   int64_t vel;
   bool state=enterCriticalSection();

   vel=velocity;
   exitCriticalSection(state);
   return vel;
}

void StepperCtrl::PrintData(void)
{
   char s[128];
   bool state=enterCriticalSection();
   sprintf(s, "%u,%u,%u", (uint32_t)numSteps,(uint32_t)getDesiredAngle(),(uint32_t)getCurrentAngle());
   SerialUSB.println(s);
   exitCriticalSection(state);
}
//this is the velocity PID feedback loop
bool StepperCtrl::vpidFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
   int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
   static int64_t lastY=getCurrentLocation();
   static int32_t lastError=0;
   static int64_t Iterm=0;
   int64_t y,z;
   int32_t v,dy;
   int64_t u;

   //get the current location
   y =currentLoc;

   v=y-lastY;

   //add in phase prediction
   z=y+calculatePhasePrediction(currentLoc);


   lastY=y;

   v=v*NZS_CONTROL_LOOP_HZ;


   if (enableFeedback) //if ((micros()-lastCall)>(updateRate/10))
   {
      int32_t error,U;
      error = velocity-v;


      Iterm += (vPID.Ki * error);
      if (Iterm>(16*4096*CTRL_PID_SCALING *motorParams.currentMa))
      {
	 Iterm=(16*4096*CTRL_PID_SCALING *motorParams.currentMa);
      }
      if (Iterm<-(16*4096*CTRL_PID_SCALING *motorParams.currentMa))
      {
	 Iterm=-(16*4096*CTRL_PID_SCALING*motorParams.currentMa);
      }

      u=((vPID.Kp * error) + Iterm - (vPID.Kd *(lastError-error)));
      U=abs(u)/CTRL_PID_SCALING/1024; //scale the error to make PID params close to 1.0;//scale the error to make PID params close to 1.0 by dividing by 1024

      if (U>motorParams.currentMa)
      {
	 U=motorParams.currentMa;
      }




      //when error is positive we need to move reverse direction
      if (u>0)
      {
	 z=z+(fullStep);
      }else
      {
	 z=z-(fullStep);

      }

      ptrCtrl->ma=U;
      ptrCtrl->angle=(int32_t)z;
      moveToAngle(z,U);
      loopError=error;
      lastError=error;
   } else
   {
      lastError=0;
      Iterm=0;
   }

   if (abs(lastError)>(systemParams.errorLimit))
   {
      return 1;
   }
   return 0;
}


//Since we are doing fixed point math our
// threshold needs to be large.
// We need a large threshold when we have fast update
// rate as well. But for most part it is random
bool StepperCtrl::pidFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
   static int count=0;

   static int32_t maxError=0;
   static int32_t lastError=0;
   static int32_t Iterm=0;
   int64_t y;

   int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;
   int32_t dy;

   y=currentLoc;

   //add in phase prediction
   y=y+calculatePhasePrediction(currentLoc);

   if (enableFeedback) //if ((micros()-lastCall)>(updateRate/10))
   {
      int64_t error,u;
      int32_t U,x;

      //error is in units of degrees when 360 degrees == 65536
      error=(desiredLoc-y); //error is currentPos-desiredPos

      Iterm+=(pPID.Ki * error);

      //Over the long term we do not want error
      // to be much more than our threshold
      if (Iterm> (motorParams.currentMa*CTRL_PID_SCALING) )
      {
	 Iterm=(motorParams.currentMa*CTRL_PID_SCALING) ;
      }
      if (Iterm<-(motorParams.currentMa*CTRL_PID_SCALING)  )
      {
	 Iterm=-(motorParams.currentMa*CTRL_PID_SCALING) ;
      }

      u=((pPID.Kp * error) + Iterm - (pPID.Kd *(lastError-error)));

      U=abs(u)/CTRL_PID_SCALING;
      if (U>motorParams.currentMa)
      {
	 U=motorParams.currentMa;
      }

      //when error is positive we need to move reverse direction
      if (u>0)
      {
	 y=y+fullStep;
      }else
      {
	 y=y-fullStep;

      }

      ptrCtrl->ma=U;
      ptrCtrl->angle=(int32_t)y;
      moveToAngle(y,U);
      loopError=error;
      lastError=error;

   }else
   {
      lastError=0;
      Iterm=0;
   }

   if (abs(lastError)>(systemParams.errorLimit))
   {
      return 1;
   }
   return 0;
}



// the phase prediction tries to predict error from sensor based
// on current location and previous location.
// TODO our error can help in the phase prediction.
// if the error
int64_t StepperCtrl::calculatePhasePrediction(int64_t currentLoc)
{
   static int64_t lastLoc=0;
   static int32_t mean=0;
   int32_t dx,x;

#ifndef ENABLE_PHASE_PREDICTION
   return 0;
#endif

   //what was our change in the location
   dx=currentLoc-lastLoc;  //max value is typically less than 327(1.8 degrees) or 163(0.9 degree)

   //if the motor direction changes,  zero phase prediction
   if (SIGN(dx) != SIGN(mean))
   {
      //last thing we want is phase prediction during direction change.
      mean=0;
   } else
   {
      if (abs(dx)>abs(mean))
      {
	 //increase mean really slowly, 2048 ~ 1/3 second with 6khz processing loop
	 // in fixed point since the dx is so small we need to scale it up to average
	 // dx has be be greater than 8 to change the mean...
	 // this limits the acceleration of motor above max processing speed (6k*1.8)=1800RPM
	 //  however I doubt the motor can accelerate that fast with any load...
	 //  The average helps prevent external impulse error from causing prediction to cause issues.
	 mean=DIVIDE_WITH_ROUND(2047*mean + dx*128, 2048);
      }else
      {
	 //decrease fast
	 //do not add more phase prediction than the difference in last two samples.
	 mean=dx*128;
      }
   }
   lastLoc=currentLoc;

   x= mean/128; //scale back to normal
   return x;
}

//this was written to do the PID loop not modeling a DC servo
// but rather using features of stepper motor.
bool StepperCtrl::simpleFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
   static uint32_t t0=0;
   static uint32_t calls=0;

   static int32_t maxError=0;
   static int32_t lastError=0;
   static int32_t i=0;
   static int32_t iTerm=0;
   //static int64_t lastY=getCurrentLocation();
   static int32_t velocity=0;

   int32_t fullStep=ANGLE_STEPS/motorParams.fullStepsPerRotation;


   int64_t y;


   //estimate our current location based on the encoder
   y=currentLoc;

   //add in phase prediction
   y=y+calculatePhasePrediction(currentLoc);


   //we can limit the velocity by controlling the amount we move per call to this function
   // this only works for velocity greater than 100rpm
   /*	if (velocity!=0)
	{
		fullStep=velocity/NZS_CONTROL_LOOP_HZ;
	}
	if (fullStep==0)
	{
		fullStep=1; //this RPM of (1*NZS_CONTROL_LOOP_HZ)/60 ie at 6Khz it is 100RPM
	}
    */
   if (enableFeedback)
   {
      int64_t error;
      int32_t u;
      int32_t ma;
      int32_t x;

      //error is in units of degrees when 360 degrees == 65536
      error=(desiredLoc-y);//measureError(); //error is currentPos-desiredPos


      //data[i]=(int16_t)error;
      //i++;
      if (i>=N_DATA)
      {
	 i=0;
      }

      if (abs(error)<fullStep)
      {
	 iTerm+=(sPID.Ki*error);
	 x=iTerm/CTRL_PID_SCALING;
      }else
      {
	 x=0;
	 iTerm=0;
      }

      if (x>fullStep)
      {
	 x=fullStep;
      }
      if (x<-fullStep)
      {
	 x=-fullStep;
      }


      u=(sPID.Kp * error)/CTRL_PID_SCALING+x+(sPID.Kd *(error-lastError))/CTRL_PID_SCALING;


      //limit error to full step
      if (u>fullStep)
      {
	 u=fullStep;
      }
      if (u<-fullStep)
      {
	 u=-fullStep;
      }

      ma=(abs(u)*(motorParams.currentMa-motorParams.currentHoldMa))
																      / fullStep + motorParams.currentHoldMa;

      //ma=(abs(u)*(NVM->SystemParams.currentMa))/fullStep;

      if (ma>motorParams.currentMa)
      {
	 ma=motorParams.currentMa;
      }



      y=y+u;
      ptrCtrl->ma=ma;
      ptrCtrl->angle=(int32_t)y;
      moveToAngle(y,ma); //35us

      lastError=error;
      loopError=error;
      //stepperDriver.limitCurrent(99);
   }

   if (abs(lastError)>(systemParams.errorLimit))
   {
      return 1;
   }
   stepperDriver.limitCurrent(99); //reduce noise on low error
   return 0;
}


void StepperCtrl::enable(bool enable)
{
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   bool feedback=enableFeedback;

   stepperDriver.enable(enable); //enable or disable the stepper driver as needed


   if (enabled==true && enable==false)
   {
      feedback = false;
   }
   if (enabled==false && enable==true) //if we are enabling previous disabled motor
   {
      feedback = true;
      setLocationFromEncoder();
   }

   enabled=enable;
   enableFeedback=feedback;
   if (state) enableTCInterrupts();
}

/*
void StepperCtrl::testRinging(void)
{
	uint16_t c;
	int32_t steps;
	int32_t microSteps=systemParams.microsteps;
	bool feedback=enableFeedback;

	enableFeedback=false;
	systemParams.microsteps=1;
	motorReset();
	for (c=2000; c>10; c=c-10)
	{
		SerialUSB.print("Current ");
		SerialUSB.println(c);
		steps+=A4954_NUM_MICROSTEPS;
		stepperDriver.move(steps,NVM->SystemParams.currentMa);
		currentLimit=false;
		measure();
	}
	systemParams.microsteps=microSteps;
	motorReset();
	enableFeedback=feedback;
}
 */

//returns -1 if no data, else returns number of data points remaining.
int32_t StepperCtrl::getLocation(Location_t *ptrLoc)
{
   bool state=enterCriticalSection();
   int32_t n;
   //check for empty
   if (locReadIndx==locWriteIndx)
   {
      //empty data
      exitCriticalSection(state);
      return -1;
   }

   //else read data
   memcpy(ptrLoc,(void *)&locs[locReadIndx], sizeof(Location_t));

   //update the read index
   locReadIndx=(locReadIndx+1)%MAX_NUM_LOCATIONS;

   //calculate number of locations left
   n=((locWriteIndx+MAX_NUM_LOCATIONS)-locReadIndx)%MAX_NUM_LOCATIONS;


   exitCriticalSection(state);
   return n;
}

void StepperCtrl::updateLocTable(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl)
{
   bool state=enterCriticalSection();
   int32_t next;

   // set the next write location
   next=(locWriteIndx+1)%MAX_NUM_LOCATIONS;

   if (next==locReadIndx)
   {
      //we are full, exit
      exitCriticalSection(state);
      //RED_LED(true); //turn Red LED on to indciate buffer full
      return;
   }

   //use ticks for the moment so we can tell if we miss data on the print.
   locs[locWriteIndx].microSecs=(int32_t)micros();
   locs[locWriteIndx].desiredLoc=(int32_t)(desiredLoc-zeroAngleOffset);
   locs[locWriteIndx].actualLoc=(int32_t)(currentLoc-zeroAngleOffset);
   locs[locWriteIndx].angle=(ptrCtrl->angle-zeroAngleOffset);
   locs[locWriteIndx].ma=ptrCtrl->ma;
   locWriteIndx=next;


   exitCriticalSection(state);
}


bool StepperCtrl::processFeedback(void)
{
   bool ret;
   int32_t us,j;
   Control_t ctrl;
   int64_t desiredLoc;
   int64_t currentLoc;
   static int64_t mean=0;;

   us=micros();

   desiredLoc=getDesiredLocation();

   currentLoc=getCurrentLocation();
   mean=(31*mean+currentLoc+16)/32;

#ifdef A5995_DRIVER //the A5995 is has more driver noise
   if (abs(currentLoc-mean)<ANGLE_FROM_DEGREES(0.9))
#else
      if (abs(currentLoc-mean)<ANGLE_FROM_DEGREES(0.3))
#endif
      {
	 currentLoc=mean;
      }



   switch (systemParams.controllerMode)
   {
      case CTRL_POS_PID:
      {
	 ret=pidFeedback(desiredLoc, currentLoc,  &ctrl);
	 break;
      }
      default:
      case CTRL_SIMPLE:
      {
	 ret=simpleFeedback(desiredLoc, currentLoc,&ctrl);
	 break;
      }
      case CTRL_POS_VELOCITY_PID:
      {
	 ret=vpidFeedback(desiredLoc, currentLoc,&ctrl);
	 break;
      }
      //TODO if disable feedback and someone switches mode
      // they will have to turn feedback back on.
      case CTRL_OFF:
      {
	 enableFeedback=false;
	 break;
      }
      case CTRL_OPEN:
      {
	 enableFeedback=false;
	 break;
      }
   }
   ticks++;
   updateLocTable(desiredLoc, currentLoc,&ctrl);
   loopTimeus=micros()-us;
   return ret;
}


//auto tuning of PID parameters based on documentation here:
// http://brettbeauregard.com/blog/2012/01/arduino-pid-autotune-library
// http://www.controleng.com/search/search-single-display/relay-method-automates-pid-loop-tuning/4a5774decc.html
void StepperCtrl::PID_Autotune(void)
{
   int32_t noiseMin, noiseMax, error;
   int32_t eMin, eMax;
   int64_t mean;
   int32_t startAngle, thres;
   int32_t i,j;
   int32_t t0,t1;
   int32_t times[100];
   int32_t angle;

   //save previous state;
   uint16_t microSteps=systemParams.microsteps;
   bool feedback=enableFeedback;
   feedbackCtrl_t prevCtrl=systemParams.controllerMode;

   //disable interrupts and feedback controller
   bool state=TC5_ISR_Enabled;
   disableTCInterrupts();
   systemParams.controllerMode=CTRL_POS_PID;
   enableFeedback=false;
   motorReset();
   //nvmWritePID(1,0,0,2);
   //set the number of microsteps to 1
   //systemParams.microsteps=1;
   for (i=0; i<10; i++)
   {
      angle=getCurrentLocation();
   }
   //	pKp=NVM->PIDparams.Kp;
   //	pKi=NVM->PIDparams.Ki;
   //	pKd=NVM->PIDparams.Kd;
   //	threshold=NVM->PIDparams.Threshold;

   //enableTCInterrupts();
   moveToAngle(angle,motorParams.currentMa);


   //moveToAngle(angle,NVM->SystemParams.currentMa);
   /*
	//next lets measure our noise on the encoder
	noiseMin=(int32_t)ANGLE_MAX;
	noiseMax=-(int32_t)ANGLE_MAX;
	mean=0;
	j=1000000UL/NZS_CONTROL_LOOP_HZ;
	prevAngle=sampleAngle();
	for (i=0; i<(NZS_CONTROL_LOOP_HZ/2); i++)
	{
		Angle a;
		a=sampleAngle();
		error=(int32_t)(prevAngle-a);

		if (error<noiseMin)
		{
			noiseMin=error;
		}

		if (error>noiseMax)
		{
			noiseMax=error;
		}
		mean=mean+(int32_t)a;
		delayMicroseconds(j);

	}
	mean=mean/i;
	while (mean>ANGLE_MAX)
	{
		mean=mean-ANGLE_STEPS;
	}
	while (mean<0)
	{
		mean=mean+ANGLE_STEPS;
	}
	//mean is the average of the encoder.
    */



   stepperDriver.move(0,motorParams.currentMa);
   delay(1000);

   //now we need to do the relay control
   for (i=0; i<10; i++)
   {
      startAngle=getCurrentLocation();
      LOG("Start %d", (int32_t)startAngle);
   }
   thres=startAngle + (int32_t)((ANGLE_STEPS/motorParams.fullStepsPerRotation)*10/9);
   LOG("Thres %d, start %d",(int32_t)thres,(int32_t)startAngle);
   eMin=(int32_t)ANGLE_MAX;
   eMax=-(int32_t)ANGLE_MAX;
   int32_t reset=0;
   int32_t force=(motorParams.currentMa);

   for (i=0; i<100; i++)
   {
      int32_t error;
      if (reset)
      {
	 motorReset();
	 stepperDriver.move(0,motorParams.currentMa);
	 delay(1000);
	 startAngle=getCurrentLocation();
	 LOG("Start %d", (int32_t)startAngle);
	 force=force-100;

	 eMin=(int32_t)ANGLE_MAX;
	 eMax=-(int32_t)ANGLE_MAX;

	 if (force<100)
	 {
	    i=100;
	    break;
	 }
	 LOG("force set to %d",force);
	 i=0;
      }
      reset=0;

      stepperDriver.move(A4954_NUM_MICROSTEPS,force);
      //moveToAngle(startAngle+(ANGLE_STEPS/motorParams.fullStepsPerRotation),force);
      //stepperDriver.move(A4954_NUM_MICROSTEPS,NVM->SystemParams.currentMa);
      t0=micros();

      error=0;
      while(error<=((ANGLE_STEPS/motorParams.fullStepsPerRotation)/2+40))
      {
	 int32_t y;
	 y=getCurrentLocation();
	 error=y-startAngle;
	 //LOG("Error1 %d",error);
	 if (error<eMin)
	 {
	    eMin=error;
	 }
	 if (error>eMax)
	 {
	    eMax=error;
	 }
	 if (abs(error)>ANGLE_STEPS/motorParams.fullStepsPerRotation*2)
	 {
	    LOG("large Error1 %d, %d, %d",error, y, startAngle);

	    reset=1;
	    break;
	 }

      }

      stepperDriver.move(0,force);

      //stepperDriver.move(0,NVM->SystemParams.currentMa);
      t1=micros();

      error=(ANGLE_STEPS/motorParams.fullStepsPerRotation);
      while(error>=((ANGLE_STEPS/motorParams.fullStepsPerRotation)/2-40))
      {
	 error=getCurrentLocation()-startAngle;
	 //LOG("Error2 %d",error);
	 if (error<eMin)
	 {
	    eMin=error;
	 }
	 if (error>eMax)
	 {
	    eMax=error;
	 }
	 if (abs(error)>ANGLE_STEPS/motorParams.fullStepsPerRotation*2)
	 {
	    LOG("large Error2 %d",error);
	    reset=1;
	    break;
	 }
      }

      times[i]=t1-t0;

   }
   for (i=0; i<100; i++)
   {
      LOG("Time %d %d",i,times[i]);
   }
   LOG("errorMin=%d",eMin);
   LOG("errorMax=%d",eMax);

   motorReset();
   systemParams.controllerMode=prevCtrl;
   systemParams.microsteps=microSteps;
   enableFeedback=feedback;
   if (state) enableTCInterrupts();

}

//void StepperCtrl::printData(void)
//{
//	bool state=TC5_ISR_Enabled;
//	disableTCInterrupts();
//	int32_t i;
//	for(i=0; i<N_DATA; i++)
//	{
//		LOG ("%d\n",data[i]);
//	}
//
//	if (state) enableTCInterrupts();
//
//}

#pragma GCC pop_options
